
#include <asm.h>

.global C_SYMBOL(WarpPerspectiveLWMMX)

C_SYMBOL(WarpPerspectiveLWMMX):


y= -0x44
DstWC  = -0x3C
Src_Y16= -0x38
Src_X16= -0x34
DstHX16= -0x30
SrcYD16= -0x2C
SrcXD16= -0x28
DstHD16= -0x24
DstHR= -0x20
DstHL= -0x1C
DstW = -0x18
SrcH = -0x14
DstPitch= -0x10
SrcPitch= -0xC
DstPt= -0x8
SrcPt= -0x4

    STMFD	SP!, {R4-R12,LR}
    SUB     SP, SP, #0x4C
Start:
//.long 0xe7fddefe
    LDR     R4, [R0,#0x0]
    STR     R4, [SP,#0x4C+SrcPt]

    LDR     R5, [R0,#0x14]
    STR     R5, [SP,#0x4C+DstW]
    STR     R5, [SP,#0x4C+DstWC]

    LDR     R9, [R0,#0x4]
    ADD     R9, R9, R5, LSL #2
    STR     R9, [SP,#0x4C+DstPt]

    LDR     R11, [R0,#0x8]
    STR     R11, [SP,#0x4C+SrcPitch]

    LDR     R10, [R0,#0xC]
    STR     R10, [SP,#0x4C+DstPitch]

    LDR     R4, [R0,#0x10]
    STR     R4, [SP,#0x4C+SrcH]

    LDR     R4, [R0,#0x18]
    STR     R4, [SP,#0x4C+DstHL]
    TMCR    wCGR1, R4


    MOV     R4, R4, LSL #16
    STR     R4, [SP,#0x4C+DstHX16]

    LDR     R4, [R0,#0x20]
    STR     R4, [SP,#0x4C+DstHD16]

    LDR     R4, [R0,#0x24]
    STR     R4, [SP,#0x4C+SrcXD16]

    LDR     R4, [R0,#0x1C]
    STR     R4, [SP,#0x4C+DstHR]

    LDR     R7, [R0,#0x28]
    STR     R7, [SP,#0x4C+SrcYD16]




    LDR	R1, =0x00000008
    TMCRR   wR15 , R1, R1

    MOV	R3, #0

    STR	R3, [SP,#0x4C+Src_X16]


Header:

    LDR	R1, [SP,#0x4C+Src_X16]
    MOV	R3, R1,	LSL#16
    MOV R8, R3, LSR#24

    LDR R4, [SP,#0x4C+SrcPt]
    ADD R4, R4, R1, LSR#14

    LDR LR, [SP,#0x4C+DstPt]
    SUB R9, LR, R5, LSL #2
    SUB LR, R9, #4


    LDR	R0, [SP,#0x4C+DstHX16]
    MOV R0, R0, LSR #16


    MOV R12, #0         //Src_Y16

    CMP	R0, #0         //DstHX
    TMCR   wCGR0, R0
    BLE Done

LoopX:
@.long 0xe7fddefe

    MOV	R1, R12, LSL#16
    MOV	R2, R12,LSR#16
    MUL     R3, R2, R11
    MOV	R1, R1, LSR#24        // R1 = v8 , R8 = u8

    ADD	    R2, R4, R3

    WLDRW wR3, [R2]
    WLDRW wR1, [R2, #4]
    ADD	R5, R2,	R11

    SUB	    R2, R4, R3

    WLDRW wR2, [R5]
    WLDRW wR0, [R5, #4]

    ADD R6, R12, R7, LSL #1
    MOV	R6, R6,LSR#16
    MUL     R5, R6, R11
    WUNPCKELUB wR3, wR3
    ADD	    R6, R4, R5
    PLD	[R6]
    PLD	[R6,R11]
    SUB	    R6, R4, R5
    PLD	[R6]
    PLD	[R6,-R11]

    WUNPCKELUB wR1, wR1

    WLDRW wR10, [R2]
    WLDRW wR8, [R2, #4]
    SUB	  R5, R2,	R11

    WUNPCKELUB wR2, wR2
    WUNPCKELUB wR0, wR0

    WLDRW wR9, [R5]
    WLDRW wR7, [R5, #4]

    TBCSTH  wR5, R8
    TBCSTH  wR6, R1

    WSUBH  wR0, wR0, wR2
    WMULUL wR0, wR0, wR5
    WSUBH  wR1, wR1, wR3
    WMULUL wR1, wR1, wR5
    WSLLH   wR2, wR2, wR15
    WSLLH   wR3, wR3, wR15
    WADDH   wR0, wR0, wR2
    WADDH   wR1, wR1, wR3

    WUNPCKELUB wR10, wR10
    WUNPCKELUB wR8, wR8
    WUNPCKELUB wR9, wR9
    WUNPCKELUB wR7, wR7

    WSUBH  wR7, wR7, wR9
    WMULUL wR7, wR7, wR5
    WSUBH  wR8, wR8, wR10
    WMULUL wR8, wR8, wR5
    WSLLH   wR9, wR9, wR15
    WSLLH   wR10, wR10, wR15
    WADDH   wR7, wR7, wR9
    WADDH   wR8, wR8, wR10

    WSRLH   wR0, wR0, wR15
    WSRLH   wR1, wR1, wR15
    WSUBH   wR0, wR0, wR1
    WMULUL  wR0, wR0, wR6
    WSLLH   wR1, wR1, wR15
    WADDH   wR0, wR0, wR1
    WSRLH   wR0, wR0, wR15
    WPACKHUS wR0, wR0, wR1

    WSRLH   wR7, wR7, wR15
    WSRLH   wR8, wR8, wR15
    WSUBH   wR7, wR7, wR8
    WMULUL  wR7, wR7, wR6
    WSLLH   wR8, wR8, wR15
    WADDH   wR7, wR7, wR8
    WSRLH   wR7, wR7, wR15
    WPACKHUS wR7, wR7, wR8

    ADD R12, R12, R7
    SUBS R0, R0, #1

    WSTRW	wR0, [R9]
    WSTRW	wR7, [LR]
    ADD     R9, R9, R10
    SUB     LR, LR, R10

    BGT	LoopX
//.long 0xe7fddefe
    TMRC  R0, wCGR0
    TMRC  R1, wCGR1
    WZERO wR0
    SUBS   R0, R1, R0

    BLE   NextLine
LoopBX:

    WSTRW	wR0, [R9]
    WSTRW	wR0, [LR]
    ADD     R9, R9, R10
    SUB     LR, LR, R10

    SUBS R0, R0, #1
    BGT	LoopBX


NextLine:

    LDR     R1, [SP,#0x4C+SrcH]
    LDR     R3, [SP,#0x4C+DstWC]
    LDR	    R5, [SP,#0x4C+DstW]
    LDR     R2, [SP,#0x4C+DstHL]
    MUL     R0, R1, R3
    SUB     R5, R5, #1
    MUL     R1, R2, R5
    LDR     R2, [SP,#0x4C+DstHR]
    SUB     R3, R3, R5
    MOV     R0, R0,LSL#16
    MLA     R1, R3, R2, R1
    BL ___udivsi3

    MOV     R7, R0

    LDR	R1, [SP,#0x4C+Src_X16]      //
    LDR	R2, [SP,#0x4C+SrcXD16]      //
    LDR R4, [SP,#0x4C+SrcPt]
    LDR LR, [SP,#0x4C+DstPt]
    ADD R1, R1, R2                  //Src_X16 += SrcXD16;
    STR	R1, [SP,#0x4C+Src_X16]
    MOV	R3, R1,	LSL#16              //
    MOV R8, R3, LSR#24              //u8 = (Src_X16 & 0xFFFF)>>8

    LDR R3, [SP,#0x4C+DstHD16]      //
    LDR	R0, [SP,#0x4C+DstHX16]      //
    ADD R4, R4, R1, LSR#14          //PIXEL* PSrcColumn=(pSrc+(Src_X16>>16))

    SUB R9, LR, R5, LSL #2          //PIXEL* dst = pDst+x
    SUB LR, R9, #4                      //PIXEL* dstup = dst

    SUB R0, R0, R3
    STR	R0, [SP,#0x4C+DstHX16]
    MOV R0, R0, LSR #16             //DstHX16 -= DstHD16;
    TMCR   wCGR0, R0

    MOV R12, #0                     //Src_Y16 = 0
    CMP R5, #0
    STR	R5, [SP,#0x4C+DstW]
    BGT  LoopX

Done:
    ADD	SP, SP,	#0x4C
    LDMFD	SP!, {R4-R12,PC}

